#include "mathtype.h"
#include "math.h"
#include "stdio.h"
    void V_assign(const VTYPE* src, VTYPE* des, const unsigned int dimension)
    {
        for (unsigned int i = 0; i < dimension; ++i)
        {
            des[i] = src[i];
        }
    }

    void V_add_V(const VTYPE* V1, const VTYPE* V2, VTYPE* des, const unsigned int dimension)
    {
        for (unsigned int i = 0; i < dimension; ++i)
        {
            des[i] = V1[i] + V2[i];
        }
    }

    void V_sub_V(const VTYPE* V1, const VTYPE* V2, VTYPE* des, const unsigned int dimension)
    {
        for (unsigned int i = 0; i < dimension; ++i)
        {
            des[i] = V1[i] - V2[i];
        }
    }

    void V_add_S(const VTYPE* V, const VTYPE s, VTYPE* des, const unsigned int dimension)
    {
        for (unsigned int i = 0; i < dimension; ++i)
        {
            des[i] = V[i] + s;
        }
    }

    void V_mul_S(const VTYPE* V, const VTYPE s, VTYPE* des, const unsigned int dimension)
    {
        for (unsigned int i = 0; i < dimension; ++i)
        {
            des[i] = V[i] * s;
        }
    }

    VTYPE V_norm(const VTYPE* V, const unsigned int dimension)
    {
        VTYPE sum_square = 0;
        for (unsigned int i = 0; i < dimension; ++i)
        {
            sum_square += V[i] * V[i];
        }
        return sqrt(sum_square);
    }

    void V_dir(const VTYPE* V, VTYPE* des, const unsigned int dimension)
    {
        V_mul_S(V, 1 / V_norm(V, dimension), des, dimension);
    }

    VTYPE V_dot_pro(const VTYPE* V1, const VTYPE* V2, const unsigned int dimension)
    {
        VTYPE result = 0;
        for (unsigned int i = 0; i < dimension; ++i)
        {
            result += V1[i] * V2[i];
        }
        return result;
    }

    void V_cross_pro(const  VTYPE V1[3], const VTYPE V2[3], VTYPE des[3])
    {
        des[0] = V1[1] * V2[2] - V1[2] * V2[1];
        des[1] = V1[2] * V2[0] - V1[0] * V2[2];
        des[2] = V1[0] * V2[1] - V1[1] * V2[0];
    }

    VTYPE V_abs(const VTYPE* V, const unsigned int dimension)
    {
        VTYPE result = 0;
        for (unsigned int i = 0; i < dimension; ++i)
        {
            result += fabs(V[i]);
        }
        return result;
    }

    void V_print(const VTYPE* V, const unsigned int dimension)
    {
        printf("[ ");
        for (unsigned int i = 0; i < dimension; ++i)
        {
            printf("%lf ", V[i]);
        }
        printf("]T\n");
    }

    void R_from_AA(const AxisAngle* AA, VTYPE R[3][3])
    {
        VTYPE sinq = sin(AA->angle), cosq = cos(AA->angle);
        VTYPE one_cosq = 1 - cosq;
        VTYPE kx = AA->axis[0], ky = AA->axis[1], kz = AA->axis[2];
        VTYPE a = kx * one_cosq, b = ky * one_cosq, c = kz * one_cosq,
            d = kx * sinq, e = ky * sinq, f = kz * sinq;
        R[0][0] = kx * a + cosq;
        R[1][0] = ky * a + f;
        R[2][0] = kz * a - e;
        R[0][1] = kx * b - f;
        R[1][1] = ky * b + cosq;
        R[2][1] = kz * b + d;
        R[0][2] = kx * c + e;
        R[1][2] = ky * c - d;
        R[2][2] = kz * c + cosq;
    }

    void R_from_axis_angle(const VTYPE axis[3], const VTYPE angle, VTYPE R[3][3])
    {
        VTYPE sinq = sin(angle), cosq = cos(angle);
        VTYPE one_cosq = 1 - cosq;
        VTYPE kx = axis[0], ky = axis[1], kz = axis[2];
        VTYPE a = kx * one_cosq, b = ky * one_cosq, c = kz * one_cosq,
            d = kx * sinq, e = ky * sinq, f = kz * sinq;
        R[0][0] = kx * a + cosq;
        R[1][0] = ky * a + f;
        R[2][0] = kz * a - e;
        R[0][1] = kx * b - f;
        R[1][1] = ky * b + cosq;
        R[2][1] = kz * b + d;
        R[0][2] = kx * c + e;
        R[1][2] = ky * c - d;
        R[2][2] = kz * c + cosq;
    }

    void R_to_AA(const VTYPE R[3][3], AxisAngle* AA)
    {
        VTYPE cosq = 0.5 * (R[0][0] + R[1][1] + R[2][2] - 1);
        if (cosq > 1)
            cosq = 1;
        if (cosq < -1)
            cosq = -1;
        AA->angle = acos(cosq);
        //printf("r to aa axis:\n");
        //printf("%lf,%lf,%lf\n",AA->axis[0],AA->axis[1],AA->axis[2]);
        if (fabs(AA->angle) < EPSR) // angle close to zero
        {
            //printf("angle close to 0\n");
            AA->angle = 0;
            for (unsigned int i = 0; i < 3; ++i)
            {
                AA->axis[i] = 0;
            }
            return;
        }
        else if (fabs(AA->angle - M_PI) < EPSR) // angle close to 180 degree
        {
            //printf("angle close to pi\n");
            AA->angle = M_PI;
            VTYPE magnitude[3];
            for (unsigned int i = 0; i < 3; ++i)
            {
                magnitude[i] = sqrt(0.5 * (R[i][i] + 1));
            }
            // determine signs
            unsigned int maxind = 0;
            if (magnitude[1] > magnitude[0])
                maxind = 1;
            if (magnitude[2] > magnitude[1])
                maxind = 2;
            if (maxind == 0)
            {
                VTYPE v1v2 = R[0][1] + R[1][0];
                VTYPE v1v3 = R[0][2] + R[2][0];
                if (v1v2 < 0.)
                    magnitude[1] *= -1;
                if (v1v3 < 0.)
                    magnitude[2] *= -1;
            }
            else if (maxind == 1)
            {
                VTYPE v1v2 = R[0][1] + R[1][0];
                VTYPE v2v3 = R[1][2] + R[2][1];
                if (v1v2 < 0.)
                    magnitude[0] *= -1;
                if (v2v3 < 0.)
                    magnitude[2] *= -1;
            }
            else if (maxind == 2) {
                VTYPE v1v3 = R[0][2] + R[2][0];
                VTYPE v2v3 = R[1][2] + R[2][1];
                if (v1v3 < 0.)
                    magnitude[0] *= -1;
                if (v2v3 < 0.)
                    magnitude[1] *= -1;
            }
            V_assign(magnitude, AA->axis, 3);
            return;
        }

        // general case
        VTYPE axis[3];
        axis[0] = R[2][1] - R[1][2];
        axis[1] = R[0][2] - R[2][0];
        axis[2] = R[1][0] - R[0][1];
        //printf("no normalized axis:\n");
        //V_print(axis,3);
        V_dir(axis, AA->axis, 3);
    }

    void R_assign(const VTYPE src[3][3], VTYPE des[3][3])
    {
        for (unsigned int i = 0; i < 3; ++i)
        {
            for (unsigned int j = 0; j < 3; ++j)
            {
                des[i][j] = src[i][j];
            }
        }
    }

    void R_mul_R(const VTYPE R1[3][3], const VTYPE R2[3][3], VTYPE des[3][3])
    {

        for (unsigned int i = 0; i < 3; ++i)
        {
            for (unsigned int j = 0; j < 3; ++j)
            {
                VTYPE sum = 0;
                for (unsigned int k = 0; k < 3; ++k)
                {
                    sum += R1[i][k] * R2[k][j];
                }
                des[i][j] = sum;
            }
        }
    }

    void R_mul_V(const VTYPE R[3][3], const VTYPE V[3], VTYPE des[3])
    {
        for (unsigned int i = 0; i < 3; ++i)
        {
            VTYPE sum = 0;
            for (unsigned int j = 0; j < 3; ++j)
            {
                sum += R[i][j] * V[j];
            }
            des[i] = sum;
        }
    }

    void R_inv(const VTYPE src[3][3], VTYPE des[3][3])
    {
        for (unsigned int i = 0; i < 3; ++i)
        {
            for (unsigned int j = 0; j < 3; ++j)
            {
                des[i][j] = src[j][i];
            }
        }
    }

    VTYPE R_abs(const VTYPE R[3][3])
    {
        VTYPE result = 0;
        for (unsigned int i = 0; i < 3; ++i)
        {
            for (unsigned int j = 0; j < 3; ++j)
            {
                result += fabs(R[i][j]);
            }
        }
        return result;
    }

    void R_print(const double R[3][3])
    {
        printf("Rotation:\n");
        printf("[ ");
        for (unsigned int i = 0; i < 3; ++i)
        {
            for (unsigned int j = 0; j < 3; ++j)
            {
                printf("%lf ", R[i][j]);
            }
            if (i == 2)
            {
                printf("]\n");
            }
            else {
                printf("\n  ");
            }
        }
    }

    void AA_print(const AxisAngle* AA)
    {
        printf("Axis Angle:\n");
        printf("Axis:\n[ ");
        for (unsigned int i = 0; i < 3; ++i)
        {
            printf("%lf ", AA->axis[i]);
        }
        printf("]\n");
        printf("Angle:\n%lf deg\n", AA->angle * RAD2DEG);
    }

    void T_from_RP(const VTYPE R[3][3], const VTYPE P[3], Transformation* des)
    {
        R_assign(R, des->rotation);
        V_assign(P, des->translation, 3);
    }

    void T_to_R(const Transformation* T, VTYPE des[3][3])
    {
        R_assign(T->rotation, des);
    }

    void T_to_P(const Transformation* T, VTYPE des[3])
    {
        V_assign(T->translation, des, 3);
    }

    void T_assign(const Transformation* src, Transformation* des)
    {
        R_assign(src->rotation, des->rotation);
        V_assign(src->translation, des->translation, 3);
    }

    void T_mul_V(const Transformation* T, const VTYPE V[3], VTYPE des[3])
    {
        VTYPE temp[3];
        R_mul_V(T->rotation, V, temp);
        V_add_V(temp, T->translation, des, 3);
    }

    void T_mul_T(const Transformation* T1, const Transformation* T2, Transformation* des)
    {
        R_mul_R(T1->rotation, T2->rotation, des->rotation);
        T_mul_V(T1, T2->translation, des->translation);
    }

    void T_inv(const Transformation* src, Transformation* des)
    {
        VTYPE RT[3][3], temp[3];
        R_inv(src->rotation, RT);
        R_mul_V(RT, src->translation, temp);
        R_assign(RT, des->rotation);
        V_mul_S(temp, -1, des->translation, 3);
    }

    VTYPE T_abs(const Transformation* T)
    {
        return R_abs(T->rotation) + V_abs(T->translation, 3);
    }

    void T_print(const Transformation* T)
    {
        printf("Transformation:\n");
        R_print(T->rotation);
        V_print(T->translation, 3);
    }

    void eye3(VTYPE R[3][3])
    {
        for (unsigned int i = 0; i < 3; ++i)
        {
            for (unsigned int j = 0; j < 3; ++j)
            {
                if (i == j)
                {
                    R[i][j] = 1;
                }
                else
                {
                    R[i][j] = 0;
                }
            }
        }
    }

    void Rx(const VTYPE angle, VTYPE R[3][3])
    {
        VTYPE costheta = cos(angle);
        VTYPE sintheta = sin(angle);
        R[0][0] = 1; R[0][1] = 0;        R[0][2] = 0;
        R[1][0] = 0; R[1][1] = costheta; R[1][2] = -sintheta;
        R[2][0] = 0; R[2][1] = sintheta; R[2][2] = costheta;
    }

    void Ry(const VTYPE angle, VTYPE R[3][3])
    {
        VTYPE costheta = cos(angle);
        VTYPE sintheta = sin(angle);
        R[0][0] = costheta;  R[0][1] = 0; R[0][2] = sintheta;
        R[1][0] = 0;         R[1][1] = 1; R[1][2] = 0;
        R[2][0] = -sintheta; R[2][1] = 0; R[2][2] = costheta;
    }

    void Rz(const VTYPE angle, VTYPE R[3][3])
    {
        VTYPE costheta = cos(angle);
        VTYPE sintheta = sin(angle);
        R[0][0] = costheta; R[0][1] = -sintheta; R[0][2] = 0;
        R[1][0] = sintheta; R[1][1] = costheta;  R[1][2] = 0;
        R[2][0] = 0;        R[2][1] = 0;         R[2][2] = 1;
    }

    void Px(const VTYPE distance, VTYPE P[3])
    {
        P[0] = distance;
        P[1] = 0;
        P[2] = 0;
    }

    void Py(const VTYPE distance, VTYPE P[3])
    {
        P[0] = 0;
        P[1] = distance;
        P[2] = 0;
    }

    void Pz(const VTYPE distance, VTYPE P[3])
    {
        P[0] = 0;
        P[1] = 0;
        P[2] = distance;
    }

    void T_Rx(const VTYPE angle, Transformation* T)
    {
        Rx(angle, T->rotation);
        Pz(0, T->translation);
    }

    void T_Ry(const VTYPE angle, Transformation* T)
    {
        Ry(angle, T->rotation);
        Pz(0, T->translation);
    }

    void T_Rz(const VTYPE angle, Transformation* T)
    {
        Rz(angle, T->rotation);
        Pz(0, T->translation);
    }

    void T_Px(const VTYPE distance, Transformation* T)
    {
        eye3(T->rotation);
        Px(distance, T->translation);
    }

    void T_Py(const VTYPE distance, Transformation* T)
    {
        eye3(T->rotation);
        Py(distance, T->translation);
    }

    void T_Pz(const VTYPE distance, Transformation* T)
    {
        eye3(T->rotation);
        Pz(distance, T->translation);
    }

    int sign(VTYPE input)
    {
        if (input < -0.000001)
        {
            return -1;
        }
        else if (input > 0.000001)
        {
            return 1;
        }
        else
        {
            return 0;
        }
    }
    unsigned int V_max(const VTYPE* V, const unsigned int dimension)
    {
        unsigned int index = 0;
        VTYPE currentmax = V[0];
        for (unsigned int i = 1; i < dimension; ++i)
        {
            if (V[i] > currentmax)
            {
                currentmax = V[i];
                index = i;
            }
        }
        return index;
    }
